{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "# HybridGaussianFactorGraph"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/hybrid/doc/HybridGaussianFactorGraph.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass  # Not running on Colab, do nothing"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "`HybridGaussianFactorGraph` is a specialized `HybridFactorGraph` designed to represent linearized hybrid systems or systems that are inherently Conditional Linear Gaussian (CLG). It primarily contains:\n",
        "\n",
        "*   `gtsam.GaussianFactor` (or derived types like `JacobianFactor`, `HessianFactor`)\n",
        "*   `gtsam.DiscreteFactor` (or derived types like `DecisionTreeFactor`, `TableFactor`)\n",
        "*   `gtsam.HybridGaussianFactor` (including `HybridGaussianConditional`)\n",
        "\n",
        "This graph type is typically the result of linearizing a `HybridNonlinearFactorGraph`. It supports specialized elimination methods (`eliminateSequential`, `eliminateMultifrontal` via `EliminateableFactorGraph` base) that correctly handle the mix of discrete and continuous variables, producing a `HybridBayesNet` or `HybridBayesTree`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {},
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "import graphviz\n",
        "\n",
        "from gtsam import (\n",
        "    JacobianFactor,\n",
        "    DecisionTreeFactor,\n",
        "    HybridGaussianFactorGraph,\n",
        "    HybridGaussianFactor,\n",
        "    GaussianConditional,\n",
        "    HybridValues,\n",
        ")\n",
        "from gtsam.symbol_shorthand import X, D"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Initialization and Adding Factors"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Hybrid Gaussian Factor Graph: \n",
            "size: 3\n",
            "Factor 0\n",
            "GaussianFactor:\n",
            "\n",
            "  A[x0] = [\n",
            "\t-1\n",
            "]\n",
            "  b = [ 0 ]\n",
            "  Noise model: unit (1) \n",
            "\n",
            "Factor 1\n",
            "DiscreteFactor:\n",
            " f[ (d0,2), ]\n",
            " Choice(d0) \n",
            " 0 Leaf  0.7\n",
            " 1 Leaf  0.3\n",
            "\n",
            "Factor 2\n",
            "HybridGaussianFactor:\n",
            "Hybrid [x1; d1]{\n",
            " Choice(d1) \n",
            " 0 Leaf :\n",
            "  A[x1] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 0 ]\n",
            "  Noise model: unit (1) \n",
            "scalar: 0\n",
            "\n",
            " 1 Leaf :\n",
            "  A[x1] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 2 ]\n",
            "isotropic dim=1 sigma=0.5\n",
            "scalar: 0\n",
            "\n",
            "}\n",
            "\n"
          ]
        }
      ],
      "source": [
        "hgfg = HybridGaussianFactorGraph()\n",
        "\n",
        "# Gaussian Factor (e.g., prior on X0)\n",
        "prior_model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n",
        "prior_factor = JacobianFactor(X(0), -np.eye(1), np.zeros(1), prior_model)\n",
        "hgfg.add(prior_factor)\n",
        "\n",
        "# Discrete Factor (e.g., prior on D0)\n",
        "dk0 = (D(0), 2) # D0 has 2 states\n",
        "discrete_prior = DecisionTreeFactor([dk0], \"0.7 0.3\") # P(D0=0)=0.7, P(D0=1)=0.3\n",
        "hgfg.add(discrete_prior)\n",
        "\n",
        "# Hybrid Gaussian Factor (e.g., measurement on X1 depends on D0)\n",
        "dk1 = (D(1), 2)\n",
        "# Measurement model 0: N(X1; 0, 1)\n",
        "gf0 = JacobianFactor(X(1), np.eye(1), np.zeros(1), gtsam.noiseModel.Isotropic.Sigma(1, 1.0))\n",
        "# Measurement model 1: N(X1; 2, 0.5)\n",
        "gf1 = JacobianFactor(X(1), np.eye(1), np.array([2.0]), gtsam.noiseModel.Isotropic.Sigma(1, 0.5))\n",
        "hybrid_meas = HybridGaussianFactor(dk1, [gf0, gf1])\n",
        "hgfg.add(hybrid_meas)\n",
        "\n",
        "hgfg.print(\"\\nHybrid Gaussian Factor Graph:\")"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "id": "dbdda9c1",
      "metadata": {},
      "outputs": [
        {
          "data": {
            "image/svg+xml": [
              "<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
              "<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
              " \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
              "<!-- Generated by graphviz version 12.0.0 (0)\n",
              " -->\n",
              "<!-- Pages: 1 -->\n",
              "<svg width=\"278pt\" height=\"84pt\"\n",
              " viewBox=\"0.00 0.00 278.00 83.60\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
              "<g id=\"graph0\" class=\"graph\" transform=\"scale(1 1) rotate(0) translate(4 79.6)\">\n",
              "<polygon fill=\"white\" stroke=\"none\" points=\"-4,4 -4,-79.6 274,-79.6 274,4 -4,4\"/>\n",
              "<!-- var7205759403792793600 -->\n",
              "<g id=\"node1\" class=\"node\">\n",
              "<title>var7205759403792793600</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"27\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"27\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">d0</text>\n",
              "</g>\n",
              "<!-- factor1 -->\n",
              "<g id=\"node6\" class=\"node\">\n",
              "<title>factor1</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"27\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var7205759403792793600&#45;&#45;factor1 -->\n",
              "<g id=\"edge2\" class=\"edge\">\n",
              "<title>var7205759403792793600&#45;&#45;factor1</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M27,-39.28C27,-25.77 27,-8.54 27,-3.96\"/>\n",
              "</g>\n",
              "<!-- var7205759403792793601 -->\n",
              "<g id=\"node2\" class=\"node\">\n",
              "<title>var7205759403792793601</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"99\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">d1</text>\n",
              "</g>\n",
              "<!-- factor2 -->\n",
              "<g id=\"node7\" class=\"node\">\n",
              "<title>factor2</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"135\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var7205759403792793601&#45;&#45;factor2 -->\n",
              "<g id=\"edge4\" class=\"edge\">\n",
              "<title>var7205759403792793601&#45;&#45;factor2</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M109.61,-40.75C118.9,-26.86 131.37,-8.22 134.34,-3.78\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352320 -->\n",
              "<g id=\"node3\" class=\"node\">\n",
              "<title>var8646911284551352320</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"243\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"243\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">x0</text>\n",
              "</g>\n",
              "<!-- factor0 -->\n",
              "<g id=\"node5\" class=\"node\">\n",
              "<title>factor0</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"243\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352320&#45;&#45;factor0 -->\n",
              "<g id=\"edge1\" class=\"edge\">\n",
              "<title>var8646911284551352320&#45;&#45;factor0</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M243,-39.28C243,-25.77 243,-8.54 243,-3.96\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352321 -->\n",
              "<g id=\"node4\" class=\"node\">\n",
              "<title>var8646911284551352321</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"171\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"171\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">x1</text>\n",
              "</g>\n",
              "<!-- var8646911284551352321&#45;&#45;factor2 -->\n",
              "<g id=\"edge3\" class=\"edge\">\n",
              "<title>var8646911284551352321&#45;&#45;factor2</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M160.39,-40.75C151.1,-26.86 138.63,-8.22 135.66,-3.78\"/>\n",
              "</g>\n",
              "</g>\n",
              "</svg>\n"
            ],
            "text/plain": [
              "<graphviz.sources.Source at 0x114e6b3e0>"
            ]
          },
          "execution_count": 4,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "# We can visualize the graph using Graphviz\n",
        "graphviz.Source(hgfg.dot())"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Key Inspection and Choosing a Gaussian Subgraph\n",
        "\n",
        "Besides the standard `keys()`, `discreteKeys()`, etc., you can `choose` a specific `GaussianFactorGraph` corresponding to a discrete assignment."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Discrete Keys: d0d1\n",
            "Continuous Keys: x0x1\n",
            "\n",
            "Chosen GaussianFactorGraph for D0=0, D1=1:\n",
            "\n",
            "size: 2\n",
            "factor 0: \n",
            "  A[x0] = [\n",
            "\t-1\n",
            "]\n",
            "  b = [ 0 ]\n",
            "  Noise model: unit (1) \n",
            "factor 1: \n",
            "  A[x1] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 2 ]\n",
            "isotropic dim=1 sigma=0.5\n"
          ]
        }
      ],
      "source": [
        "print(f\"\\nDiscrete Keys: {hgfg.discreteKeySet()}\")\n",
        "print(f\"Continuous Keys: {hgfg.continuousKeySet()}\")\n",
        "\n",
        "# Choose the GaussianFactorGraph for a specific assignment\n",
        "assignment = gtsam.DiscreteValues()\n",
        "assignment[D(0)] = 0\n",
        "assignment[D(1)] = 1 # Selects gf1 from hybrid_meas\n",
        "\n",
        "gaussian_subgraph = hgfg.choose(assignment)\n",
        "\n",
        "print(f\"\\nChosen GaussianFactorGraph for D0=0, D1=1:\")\n",
        "gaussian_subgraph.print()"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "id": "f7956872",
      "metadata": {},
      "outputs": [
        {
          "data": {
            "image/svg+xml": [
              "<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
              "<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
              " \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
              "<!-- Generated by graphviz version 12.0.0 (0)\n",
              " -->\n",
              "<!-- Pages: 1 -->\n",
              "<svg width=\"134pt\" height=\"84pt\"\n",
              " viewBox=\"0.00 0.00 134.00 83.60\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
              "<g id=\"graph0\" class=\"graph\" transform=\"scale(1 1) rotate(0) translate(4 79.6)\">\n",
              "<polygon fill=\"white\" stroke=\"none\" points=\"-4,4 -4,-79.6 130,-79.6 130,4 -4,4\"/>\n",
              "<!-- var8646911284551352320 -->\n",
              "<g id=\"node1\" class=\"node\">\n",
              "<title>var8646911284551352320</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"27\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"27\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">x0</text>\n",
              "</g>\n",
              "<!-- factor0 -->\n",
              "<g id=\"node3\" class=\"node\">\n",
              "<title>factor0</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"27\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352320&#45;&#45;factor0 -->\n",
              "<g id=\"edge1\" class=\"edge\">\n",
              "<title>var8646911284551352320&#45;&#45;factor0</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M27,-39.28C27,-25.77 27,-8.54 27,-3.96\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352321 -->\n",
              "<g id=\"node2\" class=\"node\">\n",
              "<title>var8646911284551352321</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"99\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">x1</text>\n",
              "</g>\n",
              "<!-- factor1 -->\n",
              "<g id=\"node4\" class=\"node\">\n",
              "<title>factor1</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"99\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352321&#45;&#45;factor1 -->\n",
              "<g id=\"edge2\" class=\"edge\">\n",
              "<title>var8646911284551352321&#45;&#45;factor1</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M99,-39.28C99,-25.77 99,-8.54 99,-3.96\"/>\n",
              "</g>\n",
              "</g>\n",
              "</svg>\n"
            ],
            "text/plain": [
              "<graphviz.sources.Source at 0x114e6b6e0>"
            ]
          },
          "execution_count": 6,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "# Note: The discrete factor is ignored in the chosen graph.\n",
        "# The HybridGaussianFactor contributes its gf1 component.\n",
        "# The prior on X0 is included.\n",
        "graphviz.Source(gaussian_subgraph.dot())"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Computing Errors (`error`, `errorTree`, `probPrime`, `discretePosterior`)\n",
        "\n",
        "Several methods exist to evaluate the graph under different assumptions."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 7,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Total Hybrid Error (Gaussian+Discrete): 0.38167494393873247\n",
            "\n",
            "Error Tree (Gaussian factor errors):\n",
            "ErrorTree\n",
            " Choice(d1) \n",
            "ErrorTree\n",
            " 0 Choice(d0) \n",
            "ErrorTree\n",
            " 0 0 Leaf 2.5666749\n",
            "ErrorTree\n",
            " 0 1 Leaf 3.4139728\n",
            "ErrorTree\n",
            " 1 Choice(d0) \n",
            "ErrorTree\n",
            " 1 0 Leaf 0.38167494\n",
            "ErrorTree\n",
            " 1 1 Leaf 1.2289728\n"
          ]
        }
      ],
      "source": [
        "# --- Error methods ---\n",
        "hybrid_values = HybridValues()\n",
        "hybrid_values.insert(X(0), np.array([0.1]))\n",
        "hybrid_values.insert(X(1), np.array([2.1])) # Close to mean of gf1\n",
        "hybrid_values.insert(assignment) # Use D0=0, D1=1\n",
        "\n",
        "# Sum of errors (Gaussian parts) + neg log prob (Discrete parts)\n",
        "total_error = hgfg.error(hybrid_values)\n",
        "print(f\"\\nTotal Hybrid Error (Gaussian+Discrete): {total_error}\") # Should be low\n",
        "\n",
        "# Error Tree (only considers Gaussian factors, returns ADT over discrete keys)\n",
        "continuous_values = hybrid_values.continuous()\n",
        "error_tree = hgfg.errorTree(continuous_values)\n",
        "print(\"\\nError Tree (Gaussian factor errors):\")\n",
        "error_tree.print()"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 8,
      "id": "4d31e67b",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Probability Prime (unnormalized density): 0.6827169384198328\n",
            "\n",
            "Discrete Posterior Tree P(M | X=x):\n",
            "ErrorTree\n",
            " Choice(d1) \n",
            "ErrorTree\n",
            " 0 Choice(d0) \n",
            "ErrorTree\n",
            " 0 0 Leaf 0.070773923\n",
            "ErrorTree\n",
            " 0 1 Leaf 0.030331681\n",
            "ErrorTree\n",
            " 1 Choice(d0) \n",
            "ErrorTree\n",
            " 1 0 Leaf 0.62922608\n",
            "ErrorTree\n",
            " 1 1 Leaf 0.26966832\n"
          ]
        }
      ],
      "source": [
        "# probPrime (unnormalized posterior density for a full hybrid assignment)\n",
        "prob_prime = hgfg.probPrime(hybrid_values)\n",
        "print(f\"\\nProbability Prime (unnormalized density): {prob_prime}\") # exp(-error)\n",
        "\n",
        "# discretePosterior (P(M | X=x), needs only continuous values)\n",
        "discrete_posterior_tree = hgfg.discretePosterior(continuous_values)\n",
        "print(\"\\nDiscrete Posterior Tree P(M | X=x):\")\n",
        "discrete_posterior_tree.print() # ADT representing normalized discrete posterior"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Elimination (`eliminateSequential`, `eliminateMultifrontal`)\n",
        "\n",
        "Elimination methods handle the hybrid nature, producing `HybridBayesNet` or `HybridBayesTree`. Discrete keys are typically eliminated *after* continuous keys by default using `HybridOrdering`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 9,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Default Hybrid Ordering: Position 0: x0, x1, d0, d1\n",
            "\n",
            "\n",
            "Resulting HybridBayesNet:\n"
          ]
        },
        {
          "data": {
            "image/svg+xml": [
              "<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
              "<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
              " \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
              "<!-- Generated by graphviz version 12.0.0 (0)\n",
              " -->\n",
              "<!-- Pages: 1 -->\n",
              "<svg width=\"206pt\" height=\"116pt\"\n",
              " viewBox=\"0.00 0.00 206.00 116.00\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
              "<g id=\"graph0\" class=\"graph\" transform=\"scale(1 1) rotate(0) translate(4 112)\">\n",
              "<polygon fill=\"white\" stroke=\"none\" points=\"-4,4 -4,-112 202,-112 202,4 -4,4\"/>\n",
              "<!-- var7205759403792793600 -->\n",
              "<g id=\"node1\" class=\"node\">\n",
              "<title>var7205759403792793600</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"27\" cy=\"-90\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"27\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">d0</text>\n",
              "</g>\n",
              "<!-- var7205759403792793601 -->\n",
              "<g id=\"node2\" class=\"node\">\n",
              "<title>var7205759403792793601</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-90\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"99\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">d1</text>\n",
              "</g>\n",
              "<!-- var8646911284551352321 -->\n",
              "<g id=\"node4\" class=\"node\">\n",
              "<title>var8646911284551352321</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-18\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"99\" y=\"-12.95\" font-family=\"Times,serif\" font-size=\"14.00\">x1</text>\n",
              "</g>\n",
              "<!-- var7205759403792793601&#45;&gt;var8646911284551352321 -->\n",
              "<g id=\"edge1\" class=\"edge\">\n",
              "<title>var7205759403792793601&#45;&gt;var8646911284551352321</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M99,-71.7C99,-64.41 99,-55.73 99,-47.54\"/>\n",
              "<polygon fill=\"black\" stroke=\"black\" points=\"102.5,-47.62 99,-37.62 95.5,-47.62 102.5,-47.62\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352320 -->\n",
              "<g id=\"node3\" class=\"node\">\n",
              "<title>var8646911284551352320</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"171\" cy=\"-90\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"171\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">x0</text>\n",
              "</g>\n",
              "</g>\n",
              "</svg>\n"
            ],
            "text/plain": [
              "<graphviz.sources.Source at 0x114e982c0>"
            ]
          },
          "execution_count": 9,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "# Default ordering eliminates continuous then discrete\n",
        "ordering = gtsam.HybridOrdering(hgfg)\n",
        "print(f\"\\nDefault Hybrid Ordering: {ordering}\") # Expect X(0), X(1), D(0), D(1)...\n",
        "\n",
        "# Sequential elimination\n",
        "hybrid_bayes_net: gtsam.HybridBayesNet\n",
        "hybrid_bayes_net, remaining_factors = hgfg.eliminatePartialSequential(ordering)\n",
        "print(\"\\nResulting HybridBayesNet:\")\n",
        "graphviz.Source(hybrid_bayes_net.dot())"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 10,
      "id": "0340b339",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Resulting HybridBayesTree:\n"
          ]
        },
        {
          "data": {
            "image/svg+xml": [
              "<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
              "<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
              " \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
              "<!-- Generated by graphviz version 12.0.0 (0)\n",
              " -->\n",
              "<!-- Title: G Pages: 1 -->\n",
              "<svg width=\"216pt\" height=\"116pt\"\n",
              " viewBox=\"0.00 0.00 216.02 116.00\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
              "<g id=\"graph0\" class=\"graph\" transform=\"scale(1 1) rotate(0) translate(4 112)\">\n",
              "<title>G</title>\n",
              "<polygon fill=\"white\" stroke=\"none\" points=\"-4,4 -4,-112 212.02,-112 212.02,4 -4,4\"/>\n",
              "<!-- 8646911284551352320 -->\n",
              "<g id=\"node1\" class=\"node\">\n",
              "<title>8646911284551352320</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"27\" cy=\"-90\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"27\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">x0</text>\n",
              "</g>\n",
              "<!-- 7205759403792793600 -->\n",
              "<g id=\"node2\" class=\"node\">\n",
              "<title>7205759403792793600</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-90\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"99\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">d0</text>\n",
              "</g>\n",
              "<!-- 7205759403792793601 -->\n",
              "<g id=\"node3\" class=\"node\">\n",
              "<title>7205759403792793601</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"171\" cy=\"-90\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"171\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">d1</text>\n",
              "</g>\n",
              "<!-- 8646911284551352321 -->\n",
              "<g id=\"node4\" class=\"node\">\n",
              "<title>8646911284551352321</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"171\" cy=\"-18\" rx=\"37.02\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"171\" y=\"-12.95\" font-family=\"Times,serif\" font-size=\"14.00\">x1 : d1</text>\n",
              "</g>\n",
              "<!-- 7205759403792793601&#45;&gt;8646911284551352321 -->\n",
              "<g id=\"edge1\" class=\"edge\">\n",
              "<title>7205759403792793601&#45;&gt;8646911284551352321</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M171,-71.7C171,-64.41 171,-55.73 171,-47.54\"/>\n",
              "<polygon fill=\"black\" stroke=\"black\" points=\"174.5,-47.62 171,-37.62 167.5,-47.62 174.5,-47.62\"/>\n",
              "</g>\n",
              "</g>\n",
              "</svg>\n"
            ],
            "text/plain": [
              "<graphviz.sources.Source at 0x114e98410>"
            ]
          },
          "execution_count": 10,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "# Multifrontal elimination (often preferred)\n",
        "hybrid_bayes_tree = hgfg.eliminateMultifrontal(ordering)\n",
        "print(\"\\nResulting HybridBayesTree:\")\n",
        "graphviz.Source(hybrid_bayes_tree.dot())"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 5
}
